/*
 *  PotentialField.cpp
 *  powaah
 *
 *  Created by Tim Uusitalo on 4/3/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include "PotentialField.h"

using namespace MathUtility;

PotentialField::PotentialField()
{
}

PotentialField::~PotentialField()
{
}

/**
 * Inserts a charge into the field
 * The charge will be in the range of 0 to 1.
 */
void PotentialField::insertCharge(const Charge& charge, const int pos)
{
	ChargeContainer::iterator it = field.find(pos);
	
	if (it == field.end())
	{
		field[pos] = charge;
	}
	
}

/**
 * Updates the internal carstate to the current one
 */
void PotentialField::updateState(CarState& cs)
{
	current = cs;
}


/**
 * Finds the highest charge in the field
 */
const Charge PotentialField::getHighestCharge(int* const pos)
{
	Charge c(-99999.0f, 0.0f);
	for(ChargeContainer::iterator it = field.begin(); it != field.end(); it++)
	{
		if (it->second.getCharge() > c.getCharge())
		{
			c = it->second;
			if (pos)
				*pos = it->first;
		}
	}
	return c;
}

/**
 * Finds the lowest charge in the field
 */
const Charge PotentialField::getLowestCharge(int* const pos)
{
	Charge c(99999.0f, 0.0f);
	for(ChargeContainer::iterator it = field.begin(); it != field.end(); it++)
	{
		if (it->second.getCharge() < c.getCharge())
		{
			c = it->second;

			if (pos)
				*pos = it->first;
		}
	}
	return c;
	
}

/**
 * Finds the charge with the higest potential-value
 */
const Charge PotentialField::getChargeWithHighestPotential(int* const pos)
{
	Charge c(-99999.0f, 0.0f);
	
		for(ChargeContainer::iterator it = field.begin(); it != field.end(); it++)
		{
			if (it->second.getPotential() > c.getPotential())
			{
				c = it->second;
				if (pos)
					*pos = it->first;
			}
		}
		return c;
}

/**
 * @return - Finds the charge at position pos
 */
const Charge PotentialField::getChargeAt(const int pos)
{
	ChargeContainer::iterator it = field.find(pos);
	
	if (it != field.end())
	{
		return it->second;
	}
	
	return Charge();
}

void PotentialField::clear()
{
	field.clear();
}

void PotentialField::calculatePotentialForOptions(DriverOption* const options, unsigned int size)
{
	for (int i = 0; i < size; i++)
	{
		for (unsigned int j = 0; j < field.size(); j++)
		{
			float length = 0.0f;
			
			length = (options[i].getPosition() - field[j].getPosition()).length();
			
			options[i].addPotential(potential(field[j].getCharge(), length));
				
			
		}
		
		//std::cout << "index i: " << i << " potential: " << options[i].getPotential() <<  std::endl;
		
	}
	
}

void PotentialField::calculatPotentialForOptionsSquareRoot(DriverOption* const options, unsigned int size)
{
	for (int i = 0; i < size; i++)
	{
		for (unsigned int j = 0; j < field.size(); j++)
		{
			float length = 0.0f;
			
			length = (options[i].getPosition() - field[j].getPosition()).length();
			
			options[i].addPotential(potentialSquareRoot(field[j].getCharge(), length));
		}
		//std::cout << "index i: " << i << " potential: " << options[i].getPotential() <<  std::endl;
	}
}


void PotentialField::print()
{
	for (int i = 0; i < field.size(); i++)
	{
		
	}
}

const unsigned int PotentialField::size() const
{
	return field.size();
}